#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>


using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
string frame_id_string_,pc_path_name_;
ros::Publisher pose_camera_pub_, pose_lidar_pub_,pose_t_pub_;


void displayPFPose() {
    // Quaternion
//    pf_pose_mean_.setRotation(tf::Quaternion());

    // tf::Matrix3x3 m1(-1, 0, 0, 0, 1, 0, 0, 0, -1);
    // tf::Matrix3x3 m2(0, 1, 0, -1, 0, 0, 0, 0, 1);
    // m1 = m1*m2;
    // cout<<"m: "<<tf::cast(m1.getRow(0), dtype=tf.float32)<<endl;  
    // cout<<" "<<m1.getRow(1)<<endl;
    // cout<<" "<<m1.getRow(2)<<endl;
    // m1.serializeFloat()
    // cout<<"m1: "<<m1[0][0]<<" "<<m1[0][1]<<" "<<m1[0][2]<<" "<<endl;
    // cout<<" "<<m1[1][0]<<" "<<m1[1][1]<<" "<<m1[1][2]<<" "<<endl;
    // cout<<" "<<m1[2][0]<<" "<<m1[2][1]<<" "<<m1[2][2]<<" "<<endl;

//    tf::Matrix3x3 m(0,0,-1,-1,0,0,0,1,0);
//    tf::Matrix3x3 m(0, 1, 0, -1, 0, 0, 0, 0, 1);
//    tf::Matrix3x3 m(-1, 0, 0, 0, 1, 0, 0, 0, -1);

    tf::Transform camera_pose;
   tf::Quaternion q;
    Eigen::Affine3f camera_t= pcl::getTransformation(0.3, 0, 0.5, -M_PI*0.5, 0, -M_PI*0.5);
    tf::Matrix3x3 camera_m(camera_t(0,0),camera_t(0,1),camera_t(0,2),
                                        camera_t(1,0),camera_t(1,1),camera_t(1,2),
                                        camera_t(2,0),camera_t(2,1),camera_t(2,2));
    cout<< "camera  "<<camera_t(0,0)<<" "<<camera_t(0,1)<<" "<<camera_t(0,2)<<endl;
    cout<< "camera  "<<camera_t(1,0)<<" "<<camera_t(1,1)<<" "<<camera_t(1,2)<<endl;
    cout<< "camera  "<<camera_t(2,0)<<" "<<camera_t(2,1)<<" "<<camera_t(2,2)<<endl;
    camera_pose.setOrigin(tf::Vector3(camera_t(0,3),camera_t(1,3),camera_t(2,3)));
   camera_m.getRotation(q);
   camera_pose.setRotation(q);
    geometry_msgs::PoseStamped p;
    p.header.frame_id = "map";
    p.header.stamp = ros::Time::now();
    p.pose.position.x=camera_pose.getOrigin().x();p.pose.position.y=camera_pose.getOrigin().y();
    p.pose.position.z=camera_pose.getOrigin().z();
    p.pose.orientation.x=camera_pose.getRotation().x();p.pose.orientation.y=camera_pose.getRotation().y();
    p.pose.orientation.z=camera_pose.getRotation().z();p.pose.orientation.w=camera_pose.getRotation().w();
    pose_camera_pub_.publish(p);

    tf::Transform lidar_pose;
    Eigen::Affine3f lidar_t= pcl::getTransformation(0.0, 0, 0.6, 0, 0, 0);
    tf::Matrix3x3 lidar_m(lidar_t(0,0),lidar_t(0,1),lidar_t(0,2),
                                        lidar_t(1,0),lidar_t(1,1),lidar_t(1,2),
                                        lidar_t(2,0),lidar_t(2,1),lidar_t(2,2));
    lidar_pose.setOrigin(tf::Vector3(lidar_t(0,3),lidar_t(1,3),lidar_t(2,3)));
   lidar_m.getRotation(q);
   lidar_pose.setRotation(q);
    p.pose.position.x=lidar_pose.getOrigin().x();p.pose.position.y=lidar_pose.getOrigin().y();
    p.pose.position.z=lidar_pose.getOrigin().z();
    p.pose.orientation.x=lidar_pose.getRotation().x();p.pose.orientation.y=lidar_pose.getRotation().y();
    p.pose.orientation.z=lidar_pose.getRotation().z();p.pose.orientation.w=lidar_pose.getRotation().w();
    pose_lidar_pub_.publish(p);

    tf::Transform t_pose;
    Eigen::Affine3f t_t= pcl::getTransformation(0.0, -0.1, -0.3, M_PI*0.5, -M_PI*0.5, 0);
    t_t = camera_t*t_t;
    tf::Matrix3x3 t_m(t_t(0,0),t_t(0,1),t_t(0,2),
                                        t_t(1,0),t_t(1,1),t_t(1,2),
                                        t_t(2,0),t_t(2,1),t_t(2,2));
    t_pose.setOrigin(tf::Vector3(t_t(0,3),t_t(1,3),t_t(2,3)));
   t_m.getRotation(q);
   t_pose.setRotation(q);
    p.pose.position.x=t_pose.getOrigin().x();p.pose.position.y=t_pose.getOrigin().y();
    p.pose.position.z=t_pose.getOrigin().z();
    p.pose.orientation.x=t_pose.getRotation().x();p.pose.orientation.y=t_pose.getRotation().y();
    p.pose.orientation.z=t_pose.getRotation().z();p.pose.orientation.w=t_pose.getRotation().w();
    pose_t_pub_.publish(p);

}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
    pose_camera_pub_=nh.advertise<geometry_msgs::PoseStamped>("camera_pose",1);
    pose_lidar_pub_=nh.advertise<geometry_msgs::PoseStamped>("lidar_pose",1);
    pose_t_pub_=nh.advertise<geometry_msgs::PoseStamped>("t_pose",1);
    ros::Rate loop_rate(10);
    while(1){
        displayPFPose();
        loop_rate.sleep();
    }

    return 0;
}
